/***********************************************************************/
/** 
    This program makes an Eyebot robot vehicle track a coloured object by
    use of its camera and a servo.

		author Birgit Graf, UWA, 2000
  
   Heavily modified for RAS by Mattias Bratt
*/
/***********************************************************************/


#include "eyebot.h"
#include "imageproc.h"

#define SERVO_CAM SERVO11

/***********************************************************************/
/** Start program.
    The main function controlls the several states of the
    program. It always shows the input of the camera, and starts the
    main menue, where you can set several parameters like the ball
    colour or the threshold for ball recognition.
    Before starting the ball search, an RGB tou hue lookup table can
    be built (option to do so when exitting SET-->COL menu).
    This will speed upp tracking by an order of magnitude.
*/
/***********************************************************************/

int main(void) {
  
  /** flags to exit loops around switch-statements */ 
  int exit_settings = FALSE;  
  int exit_program = FALSE;	
  int exit_settings2 = FALSE;
  int exit_colour = FALSE;
   

  
  /** picture to work on */
  colimage img;
  /** pictures for LCD-output */
  image greyimg;
  image dithgreyimg;
  int key;
  BYTE servopos=128;

  /** start initialisations */
  LCDMode(SCROLLING|NOCURSOR);  /* init lcd */
  LCDClear();
  Init_Cam();			/* init camera */
  ServoHandle cam_servo= SERVOInit(SERVO_CAM); // init servo
  SERVOSet(cam_servo,servopos);
  
  do {

    LCDMenu("SET", "GO", "SND", "END");
    /* print name and ID of robot */
    LCDSetPos(0, 10);
    LCDPrintf("%s\n", OSMachineName());
    
    /* print current colour values for ball and goal */
    LCDSetPos(3, 10);
    LCDPrintf("B:%3d\n", get_ball_col());
    
    CAMGetColFrame(&img, FALSE); 
    IPColor2Grey(&img, &greyimg);
    LCDPutGraphic(&greyimg);  
    
    key = KEYRead();
    switch(key) {
    case KEY1: /* - - - MAIN--> SET - - - - */
      LCDClear();
      do {
	LCDMenu("COL", "", "...", "END");
	CAMGetColFrame(&img, FALSE); 
	IPColor2Grey(&img, &greyimg);
	LCDPutGraphic(&greyimg);  
	switch(KEYRead()) {
	case KEY1: /* SET-->COL: set colour values */
	  LCDClear();
	  do {
	    int r,g,b;
	    LCDMenu("BALL", "WHT", "RSTW", " END");
	    CAMGetColFrame(&img, FALSE);
	    IPColor2Grey(&img,&greyimg);
	    IPDither(&greyimg,&dithgreyimg);
	    // draw a crosshair over the whole picture
	    mark_object(dithgreyimg, imagerows / 2 - 1,  imagecolumns / 2 - 1, 0);
	    LCDPutGraphic(&dithgreyimg);  
	    // get colour of middle point
	    r=img[imagerows/2][imagecolumns/2][0];
	    g=img[imagerows/2][imagecolumns/2][1];
	    b=img[imagerows/2][imagecolumns/2][2];
	    // print middle point hue and target hue
	    LCDSetPos(1, 10);
	    LCDPrintf("hue%3d\n", RGBtoHue(r,g,b),get_ball_col());
	    LCDSetPos(2, 10);
	    LCDPrintf("bll%3d\n",get_ball_col());
	    // print white point-corrected RGB values
	    correct_colour(&r,&g,&b);
	    LCDSetPos(3, 10);
	    LCDPrintf("r:%3d\n",r);
	    LCDSetPos(4, 10);
	    LCDPrintf("g:%3d\n",g);
	    LCDSetPos(5, 10);
	    LCDPrintf("b:%3d\n",b);
	    
	    switch(KEYRead()) {
	    case KEY1: // BALL		
	      set_ball_colour(img);
	      break;
	    case KEY2: //WHT
	      /* Send image to PC over RS232 */
	      set_white_point(img);
	      break;
	    case KEY3: //RSTW	
	      reset_white_point(img);
	      break;
	    case KEY4: //END
	      exit_colour = TRUE; // end menu
	      // But first offer to calculate new hue lookup table
	      LCDClear();
	      LCDMenu("NO", "NO", "NO", " YES");
	      LCDSetPos(3, 0);
	      LCDPrintf("Save in colour");
	      LCDSetPos(4, 0);
	      LCDPrintf("lookup table?");
	      int pressed_key;
	      while((pressed_key=KEYRead())==0) continue;
	      if(pressed_key==KEY4) {
		compute_rgb2hueLUT();
	      }
	      break;  
	    default: break;
	    }
	  } while (!exit_colour);
	  
	  LCDClear();
	  exit_colour = FALSE;
	  break;
	  
	case KEY3: /* SET-->...: other settings */
	  LCDClear();
	  do {
	    LCDMenu("CAM", "IMG", "", "END");  
	    CAMGetColFrame(&img, FALSE);
	    IPColor2Grey(&img, &greyimg);
	    LCDPutGraphic(&greyimg);   
	    switch(KEYRead()) {
	    case KEY1: set_cam_parameters();
	      break;		/* CAM: set camera values */
	    case KEY2: set_img_parameters();
	      break;		/* IMG: set image parameters */
	    case KEY3: 
	      break;
	    case KEY4: exit_settings2 = TRUE;
	      break;		/* END */
	    default: break;
	    }
	  } while (!exit_settings2);
	  
	  LCDClear();
	  exit_settings2 = FALSE;
	  break;
	  
	case KEY4: exit_settings = TRUE; break; /* SET-->END */
	default: break;
	}
      } while (!exit_settings);
      LCDClear();
      exit_settings = FALSE; break;
      

      case KEY2: /* - - - - MAIN-->GO - - - - - */
      LCDClear();
      LCDMenu("SND", "", "STOP", "");
      
      while((key=KEYRead())!=KEY3){
	int row,col,size, error;					
	CAMGetFrameRGB((BYTE *)img);
	IPColor2Grey(&img,&greyimg);
	IPDither(&greyimg,&dithgreyimg);
	if(find_object(&row,  &col, &size, &error, get_ball_col(), img)) {
	  // show target position in image
	  mark_object(dithgreyimg, row, col, size);
	  // do simple (and slow!!) tracking of target using a servo
	  // (camera needs to be mounted on the servo)
	  if(col<35) servopos+=3;
	  if(col>45) servopos-=3;
	  SERVOSet(cam_servo,servopos);
	}
	else {
	  row=col=size=0;
	  error=99;
	}
	LCDSetPos(0, 10);
	LCDPrintf("r:%2d\n", row);
	LCDSetPos(1, 10);
	LCDPrintf("c:%2d\n", col);
	LCDSetPos(2, 10);
	LCDPrintf("sz:%2d\n", size);
	LCDSetPos(3, 10);
	LCDPrintf("er:%2d\n", error);
	LCDPutGraphic(&dithgreyimg);    	
	if(key==KEY1) {
	  sendRGBData(img);
	  LCDClear();
	  LCDMenu("SND", "", "STOP", "");
	}
      }
      LCDClear();
      
      break;
    case KEY3: /* - - - - GO-->SND - - - - */
      sendRGBData(img);
      break;
    case KEY4: exit_program = TRUE; break;      /* - - - - GO-->STOP - - - - */
    default: break;
    }
  } while (!exit_program);
  
  SERVORelease(cam_servo);
  return 0;
}


